'''
                           _ooOoo_
                         o8888888o
                           88" . "88
                            (| ^_^ |)
                           O\  =  /O
                      ____/`---'\____
                          .'  \\|     |//  `.
                         /  \\|||  :  |||//  \
                       /  _||||| -:- |||||-  \
                        |   | \\\  -  /// |   |
                       | \_|  ''\---/''  |   |
                     \  .-\__  `-`  ___/-. /
                  ___`. .'  /--.--\  `. . ___
           ."" '<  `.___\_<|>_/___.'  >'"".
                | | :  `- \`.;`\ _ /`;.`/ - ` : | |
              \  \ `-.   \_ __\ /__ _/   .-` /  /
         佛祖保佑       永无BUG     永不修改
 '''

import sensor, image, time, ustruct
from pyb import UART, LED
import pyb

THRESHOLD = (13, 96, 84, -20, -36, 127)#阈值

#补光程序
LED(1).on()
LED(2).on()
LED(3).on()

#摄像头初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA)
sensor.skip_frames(time = 2000)
sensor.set_vflip(1) # 打开垂直翻转
sensor.set_hmirror(1) # 打开水平镜像
clock = time.clock()

#uart初始化
uart = UART(3,115200)
uart.init(115200, bits=8, parity=None, stop=1)

#识别区域
roil = [(0, 17, 15, 25),#左
        (65, 17, 15, 25),#右
        (30, 0, 20, 15)]#上

#数据打包函数
#00直线
#01十字
#10上左
#11上右
#12停车
#x为截距，a为斜率
def outuart(x,a,flag):
    global uart
    f_x=0
    f_a=0
    if flag==1:#十字
        x,a,f_x,f_a=(0,0,0,1)
    if flag==2:#上左
        x,a,f_x,f_a=(0,0,1,0)
    if flag==3:#上右
        x,a,f_x,f_a=(0,0,1,1)
    if flag==4:#停车
        x,a,f_x,f_a=(0,0,1,2)
    print(f_x)
    print(f_a)

    #打包帧
    #data = ustruct.pack("<bbhhhhbb",#格式为两个字符两个短整型一个字符
                        #0x2C,#帧头1
                        #0x12,#帧头2
                        #int(34),
                        #int(35),
                        #int(36),
                        #int(37),
                        #int(x),#数据1
                        #int(a),#数据2
                        #int(f_x),#数据1
                        #int(f_a),#数据2
                        #0X0A,
                        #0X0D)#帧尾

    #发送数据
    #uart.write(data)
    uart.write("@")
    uart.write(str(int(x)))
    uart.write(",")
    uart.write(str(a))
    uart.write(",")
    uart.write(str(f_x))
    uart.write(",")
    uart.write(str(f_a))
    uart.write(",")
    uart.write('\r')
    uart.write('\n')


#主函数
while(True):
    left_flag = 0
    right_flag = 0
    up_flag = 0
    clock.tick()
    img = sensor.snapshot().binary([THRESHOLD])#二值化
    line = img.get_regression([(100,100)], robust = True)#线性回归

    #画出左右上的边框
    for rec in roil:
        img.draw_rectangle(rec, color=(255,0,0))


    if (line):
        #线性回归偏差量，x轴正方向为正，负方向为负
        rho_err = abs(line.rho())-img.width()/2
        #线性回归坐标调整，y轴正方向方向为0度，向右逐渐变大，向左逐渐变小
        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()

        img.draw_line(line.line(), color = 127)#直线绘画

        #线性回归成功
        if line.magnitude()>8:
            outdata=[rho_err,theta_err,0]
            print(outdata)
            outuart(rho_err,theta_err,0)
            if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=roil[0]):#left
                #print('left')
                left_flag=1
            if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=roil[1]):#right
                #print('right')
                right_flag=1
            if img.find_blobs([(96, 100, -13, 5, -11, 18)],roi=roil[2]):#up
                #print('up')
                up_flag=1
            #路口判断
            if left_flag==1 and right_flag==1:#十字路口
                outuart(0,0,1)
                print('shizi')
                continue
            if left_flag==1 and up_flag==1:#左T路口
                outuart(0,0,2)
                print('up-left')
                continue
            if right_flag==1 and up_flag==1:#右T路口
                outuart(0,0,3)
                print('up-right')
                continue
        #线性回归失败
        else:
            pass
    #循迹结束
    else:
        outuart(0,0,4)
        print('stop')
